/**
 * @file math_utils.h
 * @brief Math utilities for H743-V2 Flight Control System
 */

#ifndef __MATH_UTILS_H
#define __MATH_UTILS_H

#ifdef __cplusplus
extern "C" {
#endif

#include <stdint.h>
#include <math.h>

#define PI 3.14159265358979323846f
#define TWO_PI (2.0f * PI)
#define HALF_PI (PI / 2.0f)
#define GRAVITY 9.80665f

/* Vector3 structure */
typedef struct {
    float x;
    float y;
    float z;
} Vector3_t;

/* Quaternion structure */
typedef struct {
    float w;
    float x;
    float y;
    float z;
} Quaternion_t;

/* Matrix3x3 structure */
typedef struct {
    float m[3][3];
} Matrix3x3_t;

/* Math functions */
float constrain_float(float val, float min, float max);
float wrap_pi(float angle);
float wrap_2pi(float angle);

/* Vector operations */
void vector3_add(const Vector3_t *a, const Vector3_t *b, Vector3_t *result);
void vector3_sub(const Vector3_t *a, const Vector3_t *b, Vector3_t *result);
float vector3_dot(const Vector3_t *a, const Vector3_t *b);
void vector3_cross(const Vector3_t *a, const Vector3_t *b, Vector3_t *result);
float vector3_length(const Vector3_t *v);
void vector3_normalize(Vector3_t *v);

/* Quaternion operations */
void quaternion_from_euler(float roll, float pitch, float yaw, Quaternion_t *q);
void quaternion_to_euler(const Quaternion_t *q, float *roll, float *pitch, float *yaw);

#ifdef __cplusplus
}
#endif

#endif /* __MATH_UTILS_H */

